import rclpy
from rclpy.node import Node
from base_interfaces_demo.srv import FaceDetectors
import face_recognition
import cv2
from ament_index_python.packages import get_package_share_directory #获取功能包share目录绝对路径
import os
from cv_bridge import CvBridge
import time

class FaceDetectService(Node):
    def __init__(self):
        super(). __init__('face_detection_service_node')   
        self.service_=self.create_service(FaceDetectors,'/face_detection',self.detection_face_callback)
        self.bridge=CvBridge()
        self.number_of_times_to_upsample=1
        self.model='hog'
        self.default_image_path=os.path.join(get_package_share_directory("py02_service"),"resource/default.jpg")
        self.get_logger().info("人脸检测服务已经启动！！！")
    def detection_face_callback(self,request,response):
        if request.image.data:
            cv_image=self.bridge.imgmsg_to_cv2(request.image)
        else:
            cv_image=cv2.imread(self.default_image_path)
            self.get_logger().info(f"传入图像为空，使用默认图像")
        start_time=time.time()
        self.get_logger().info(f"加载完成图像，开始识别")
        face_locations=face_recognition.face_locations(cv_image,number_of_times_to_upsample =self.number_of_times_to_upsample,model=self.model)
        response.use_time=time.time()-start_time
        response.number=len(face_locations)
        for top,right,bottom,left in face_locations:
            response.top.append(top)
            response.bottom.append(bottom)
            response.left.append(left)
            response.right.append(right)
        return response
def main():
    rclpy.init()
    rclpy.spin(FaceDetectService())
    rclpy.shutdown()
    

if __name__=='__main__':
    main()

